using System;
using System.Diagnostics;

namespace OpenRay
{
	//Immutable class
	public class Matrix
	{
		public static readonly Matrix Identity = new Matrix( 1f, 0f, 0f, 0f,
			                   			  		      	 	 0f, 1f, 0f, 0f,
			                   					      	     0f, 0f, 1f, 0f,
			                   					      		 0f, 0f, 0f, 1f );
		
		private float[] m = new float[16];
		
		private const double Delta = 0.00005;
		
		public override bool Equals (object obj)
		{
			if ( obj == null )
				return false;
			
			Matrix mat = obj as Matrix;
			
			if ( (Object)m == null )
				return false;
			
			for ( int i = 0; i < m.Length; i++ ) {
				if ( Math.Abs( m[i] - mat.m[i] ) > Delta )
					return false;
			}
			
			return true;
		}
		
		public override int GetHashCode()
		{
			int ret = 0;
			
			for( int i = 0; i < m.Length; i++ ) 
			{
				ret += (int)m[i];
			}
			
			return ret;
		}
		
		public override string ToString()
		{
			string ret = "";
			
			ret += "\n";
			ret += m[0].ToString() + "\t" + m[4].ToString() + "\t" + m[8].ToString() + "\t" + m[12].ToString() + "\t" + "\n";
			ret += m[1].ToString() + "\t" + m[5].ToString() + "\t" + m[9].ToString() + "\t" + m[13].ToString() + "\t" + "\n";
			ret += m[2].ToString() + "\t" + m[6].ToString() + "\t" + m[10].ToString() + "\t" + m[14].ToString() + "\t" + "\n";
			ret += m[3].ToString() + "\t" + m[7].ToString() + "\t" + m[11].ToString() + "\t" + m[15].ToString() + "\t" + "\n";
			
			return ret;
		}

		private Matrix() { }
		private Matrix( Matrix mat )
		{
			Array.Copy( mat.m, m, 16 );	
		}
		
		public Matrix ( float m00, float m10, float m20, float m30,
		                float m01, float m11, float m21, float m31,
		                float m02, float m12, float m22, float m32,
		                float m03, float m13, float m23, float m33 )
		{
			m[0] = m00;
			m[1] = m01;
			m[2] = m02;
			m[3] = m03;
			
			m[4] = m10;
			m[5] = m11;
			m[6] = m12;
			m[7] = m13;
			
			m[8] = m20;
			m[9] = m21;
			m[10] = m22;
			m[11] = m23;
			
			m[12] = m30;
			m[13] = m31;
			m[14] = m32;
			m[15] = m33;
		}
		
		public static Matrix CreateTranslate( float x, float y, float z ) 
		{
			return new Matrix( 1f, 0f, 0f, x,
			                   0f, 1f, 0f, y,
			                   0f, 0f, 1f, z,
			                   0f, 0f, 0f, 1f );
		}
		
		public static Matrix CreateScale( float x, float y, float z )
		{
			return new Matrix( x, 0f, 0f, 0f,
			                   0f, y, 0f, 0f,
			                   0f, 0f, z, 0f,
			                   0f, 0f, 0f, 1f );	
		}
		
		public static Matrix CreateRotate( float theta, float x, float y, float z )
		{
    		float length = x * x + y * y + z * z;
			Debug.Assert( length != 0f );
			
		    if( length > 1.000005f || length < 0.999995f ) {
		        length = 1.0f / (float)Math.Sqrt( length );
		
		        x *= length;
		        y *= length;
		        z *= length;
		    }

    		float s = (float)Math.Sin( theta * Math.PI / 180f );
    		float c = (float)Math.Cos( theta * Math.PI / 180f );
    		float ic = 1.0f - c;

    		float m00 = x * x * ic + c;
    		float m01 = x * y * ic - z * s;
    		float m02 = x * z * ic + y * s;

    		float m10 = y * x * ic + z * s;
    		float m11 = y * y * ic + c;
    		float m12 = y * z * ic - x * s;

   			float m20 = x * z * ic - y * s;
    		float m21 = y * z * ic + x * s;
    		float m22 = z * z * ic + c;

    		return new Matrix( m00, m01, m02, 0f,
                        	   m10, m11, m12, 0f,
                        	   m20, m21, m22, 0f,
                          	   0f,   0f,  0f, 1f );	
		}
		
		public static Matrix CreateOrtho( float left, float right, float bottom, float top, float nearVal, float farVal )
		{
 		    Debug.Assert( left != right );
			Debug.Assert( top != bottom );
			Debug.Assert( nearVal != farVal ); 
			
			float tx = -( right + left ) / ( right - left );
			float ty = -( top + bottom ) / ( top - bottom );
			float tz = -( farVal + nearVal ) / ( farVal - nearVal );
			
			float sx = 2f / ( right - left );
			float sy = 2f / ( top - bottom );
			float sz = -2f / ( farVal - nearVal );
			
			return new Matrix ( sx, 0f, 0f, tx,
			                    0f, sy, 0f, ty,
			                    0f, 0f, sz, tz,
			                    0f, 0f, 0f, 1f );
		}
		
		public static Matrix CreateFrustum( float left, float right, float bottom, float top, float nearVal, float farVal )
		{
 		    Debug.Assert( left != right );
			Debug.Assert( top != bottom );
			Debug.Assert( nearVal != farVal ); 

    		float sx = ( 2.0f * nearVal ) / ( right - left );
    		float sy = ( 2.0f * nearVal ) / ( top - bottom );

    		float a = ( right + left ) / ( right - left );
    		float b = ( top + bottom ) / ( top - bottom );

    		float c = -1.0f * ( farVal + nearVal ) / ( farVal - nearVal );
    		float d = ( -2.0f * farVal * nearVal ) / ( farVal - nearVal );

			return new Matrix( sx, 0f,  a,  0f, 
			                   0f, sy,  b,  0f, 
			                   0f, 0f,  c,  d,
			                   0f, 0f, -1f, 0f );
		}
		
		public static Matrix operator * ( Matrix m1, Matrix m2 )
		{
			Matrix ret = new Matrix();
			
			for( int i = 0; i < 4; i++ ) 
			{
				for ( int j = 0; j < 4; j++ ) {
					ret.m[i * 4 + j] = m1.m[0 * 4 + j] * m2.m[i * 4 + 0] +	
					           		   m1.m[1 * 4 + j] * m2.m[i * 4 + 1] +
							   		   m1.m[2 * 4 + j] * m2.m[i * 4 + 2] +
							   		   m1.m[3 * 4 + j] * m2.m[i * 4 + 3];
				}
			}
			
			return ret;
		}
		
		private void divideRow( uint row, float f ) {
    		float divisor = 1f / f;
	
    		m[0 * 4 + row] *= divisor;
			m[1 * 4 + row] *= divisor;
			m[2 * 4 + row] *= divisor;
			m[3 * 4 + row] *= divisor;
		}
		
		private void scaledSubtractRow( uint constantRow, float f, uint modifiedRow ) {
    		m[0 * 4 + modifiedRow] -= m[0 * 4 + constantRow] * f;
   			m[1 * 4 + modifiedRow] -= m[1 * 4 + constantRow] * f;
    		m[2 * 4 + modifiedRow] -= m[2 * 4 + constantRow] * f;
    		m[3 * 4 + modifiedRow] -= m[3 * 4 + constantRow] * f;
		}
		
		private void swapRows( uint row, uint row2 ) {
    		for ( int i = 0; i < 4; i++ ) {
        		float temp = m[i * 4 + row];
        		m[i * 4 + row] = m[i * 4 + row2];
        		m[i * 4 + row2] = temp;
    		}
		}
		
		public static Matrix Inverse( Matrix input ) {
		    Matrix m = new Matrix( input );
			Matrix ret = new Matrix( Matrix.Identity );
			
		    for( uint r = 0, lead = 0; r < 4; r++ ) {
		        if ( lead >= 4 )
		            break;
		
		        uint i = r;
		
		        while ( m.m[lead * 4 + i] == 0 ) {
		            i++;
		
		            if ( i == 4 ) {
		                i = r;
		                lead++;
		
		                if ( lead == 4 ) {
		                    return ret;
		                }
		            }
		        }
		
		        if ( i != r ) {
					m.swapRows(i, r);
					ret.swapRows(i, r);
		        }
		
		        ret.divideRow( r, m.m[lead * 4 + r] );
		        m.divideRow( r, m.m[lead * 4 + r] );
		
		        for ( i = 0; i < 4; i++ ) {
		            if ( i != r ) {
						ret.scaledSubtractRow( r, m.m[lead * 4 + i], i );
						m.scaledSubtractRow( r, m.m[lead * 4 + i], i );
		            }
		        }
		        lead++;
		    }
			
			return ret;
		}
		
		public static Point operator * (Matrix m, Point p)
		{
			return new Point(p.X * m.m[0] + p.Y * m.m[4] + p.Z * m.m[8] + m.m[12],
			                 p.X * m.m[1] + p.Y * m.m[5] + p.Z * m.m[9] + m.m[13],
			                 p.X * m.m[2] + p.Y * m.m[6] + p.Z * m.m[10] + m.m[14],
			                 p.X * m.m[3] + p.Y * m.m[7] + p.Z * m.m[11] + m.m[15] );
		}
		
		public static Vector operator * (Matrix m, Vector v)
		{			
			return new Vector(v.X * m.m[0] + v.Y * m.m[4] + v.Z * m.m[8],
			                  v.X * m.m[1] + v.Y * m.m[5] + v.Z * m.m[9],
			                  v.X * m.m[2] + v.Y * m.m[6] + v.Z * m.m[10] );
		}
		
		public static Vector TransposedMultiply(Matrix m, Vector v)
		{			
			return new Vector(v.X * m.m[0] + v.Y * m.m[1] + v.Z * m.m[2],
			                  v.X * m.m[4] + v.Y * m.m[5] + v.Z * m.m[6],
			                  v.X * m.m[8] + v.Y * m.m[9] + v.Z * m.m[10] );
		}
	}
}

